#include "includes.h"
#include "start.h"

char gramb_version[]="0.048a 2014-04-13 19:24";

extern FSM gramb;
extern FSM_EVENT gramb_event;
extern int mcg_clk_khz,core_clk_khz,periph_clk_khz;

extern control_struct servo,motor[2],anti[2];

extern const uint8_t amp_ad[AMP_ADC_CHN+1];
extern uint16 amp[4];

void print_version(void){
    printf("\n\nHIT_Gram[b] ver %s\nmcg_clk_khz=%dkhz,\ncore_clk_khz=%dkhz,\nperiph_clk,khz=%dkhz\n", gramb_version, mcg_clk_khz, core_clk_khz, periph_clk_khz);
}

uint16 CRC_CHECK(uint16 *buf, int cnt){
    int i,j;
    uint16 temp=0xFFFF;

    for(i=0; i<cnt; ++i){
        temp^=buf[i];
        for(j=0; j<8; ++j){
            if(temp&0x01)
                temp=(temp>>1)^0xA001;
            else
                temp=temp>>1;
        }
    }
    return temp;
}

void vscope_data_feedback(int *amp, int *cnt){
    int i,tmp;
    uint16 buff[10],CRC_check;
    buff[0]=(uint16)amp[0]&0xFF;
    buff[1]=(uint16)(amp[0]>>8)&0xFF;
    tmp=(int)motor[0].s;
    buff[2]=(uint16)tmp&0xFF;
    buff[3]=(uint16)(tmp>>8)&0xFF;
    buff[4]=(uint16)cnt[0]&0xFF;
    buff[5]=(uint16)(cnt[1]>>8)&0xFF;
    buff[6]=(uint16)cnt[1]&0xFF;
    buff[7]=(uint16)(cnt[1]>>8)&0xFF;
    CRC_check=CRC_CHECK(buff, 8);
    buff[8]=(uint16)CRC_check&0xFF;
    buff[9]=(uint16)(CRC_check>>8)&0xFF;

    for(i=0; i<10; ++i)
        uart0_putchar(UART0_BASE_PTR, buff[i]);
}

void sort(int *array, int n){
    /* Selection sort */
    int i,j,p;
    int max;
    for(i=0; i<n; ++i){
        p=i;
        max=array[i];
        for(j=i+1; j<n; ++j)
            if(max<array[j]){
                max=array[j];
                p=j;
            }
        if(p!=i){
            array[p]=array[i];
            array[i]=max;
        }
    }
}

void dtoc(double d){
    int i,j=0;
    char string[20];

    if(d<0){
        string[0]='-';
        d=-d;
        ++j;
    }
    i=(int)d;
    sprintf(string+j, "%d.%04d", i, (int)((d-i)*10000));

    printf("%s", string);
}

uint16 read_input(void){
    int j;
    uint16 i=0;
    static int in[6][2]={{GPIO_PORTC, 1}, {GPIO_PORTD, 5}, {GPIO_PORTD, 4}, {GPIO_PORTD, 3}, {GPIO_PORTD, 2}, {GPIO_PORTD, 1}};
    for(j=0; j<6; ++j)
        i|=gpio_read(in[j][0], in[j][1])<<j;
    return i;
}

void dma_adc_activate(void){
    DMA_CLEAR_FLAG(DMA_CH0);
    DMA_CLEAR_FLAG(DMA_CH1);

    DMA_DAR_SET(DMA_CH0, (uint32_t)&amp);
    DMA_SAR_SET(DMA_CH1, (uint32_t)&amp_ad);
    DMA_BCR_SET(DMA_CH0, DMA_ADC_CHN<<1);
    DMA_BCR_SET(DMA_CH1, DMA_ADC_CHN+1);

    DMA_START(DMA_CH1);
}
